#include "kernel_functions.h"
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;

int main(int argc, char **argv)
{
    int cmd_delete_last = system("rm ../image_rect -rf");
    cv::waitKey(1000);
    int cmd_new_dir = system("mkdir ../image_rect"); // 当前仅保存左目相机
    cv::waitKey(1000);
    // 配置文件 & 左相机图像 & 右相机图像 & 时间戳文本 路径
    string strPathConfig, strPathLeft, strPathRight, strPathTimes;
    strPathConfig = "/home/mxg/MySpace/ORB_SLAM2_detailed_comments/Examples/Stereo/EuRoC.yaml";
    strPathLeft = "/home/mxg/MySpace/catkin_ws/DataSets/Euroc/V1_01_easy/mav0/cam0/data";
    strPathRight = "/home/mxg/MySpace/catkin_ws/DataSets/Euroc/V1_01_easy/mav0/cam1/data";
    strPathTimes = "/home/mxg/MySpace/ORB_SLAM2_detailed_comments/Examples/Stereo/EuRoC_TimeStamps/V101_B.txt";

    // step 1 获取图像的访问路径
    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimeStamp;
    int32_t flag_step_load_images = step_load_images(strPathLeft, strPathRight, strPathTimes, vstrImageLeft, vstrImageRight, vTimeStamp);
    if(flag_step_load_images == 0)
    {
        printf("step 1: load images successful\n");
        printf("vstrImageLeft: %ld | vstrImageRight: %ld | vTimeStamp: %ld\n", vstrImageLeft.size(), vstrImageRight.size(), vTimeStamp.size());
    }
    else{
        printf("step 1: Fail to load images !\n");
        return -1;
    }

    // step 2 从给出的配置文件中读取去畸变参数
    //存储四个映射矩阵
    cv::Mat M1l,M2l,M1r,M2r;
    step_read_configs(strPathConfig, M1l, M2l, M1r, M2r);

    // 获取图像数目
    const int nImages = vstrImageLeft.size();

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    // step 4 对每一张输入的图像执行追踪
    cv::Mat imLeft, imRight, imLeftRect, imRightRect;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read left and right images from file
        // step 4.1 读取原始图像
        imLeft = cv::imread(vstrImageLeft[ni],CV_LOAD_IMAGE_UNCHANGED);
        imRight = cv::imread(vstrImageRight[ni],CV_LOAD_IMAGE_UNCHANGED);

        step_calibrate_stereo_imgs(
            vstrImageLeft[ni], vstrImageRight[ni], 
            imLeft, imRight, 
            imLeftRect, imRightRect, 
            M1r,M2r, cv::INTER_LINEAR);

        // 将数据保存
        if(imLeftRect.empty() || imRightRect.empty())
        {
            cerr << endl << "Error in Rectified images: " 
                 << imLeftRect << endl;
            return -1;
        }
        // 新文件名，仅 左目相机
        string strImageLeftRect = "../image_rect/" + vstrImageLeft[ni].erase(0, 69);
        cout << strImageLeftRect << endl;
        // 保存文件
        cout << cv::imwrite(strImageLeftRect, imLeftRect) << endl;
        // cout << "I am saving at : " << ni << endl;
        cv::waitKey(200);
    }

    cout << "Ok, Done !! All the rectified images are saved in " << "../image_rect/" << endl << endl;

    return 0;
}

